#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <tf/transform_listener.h>
#include <memory>
#include "auto_navigation/planning/pointcloud_planner.hpp"

class PointCloudPlanningNode {
public:
    PointCloudPlanningNode() : nh_(), pnh_("~"), tf_listener_() {
        // 创建规划器
        planner_ = std::make_shared<auto_navigation::planning::PointCloudPlanner>();
        
        // 初始化规划器
        if (!planner_->initialize(pnh_)) {
            ROS_ERROR("Failed to initialize PointCloudPlanner");
            return;
        }
        
        // 订阅点云地图
        map_sub_ = nh_.subscribe("/static_map_cloud", 1, &PointCloudPlanningNode::mapCallback, this);
        
        // 订阅目标点
        goal_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &PointCloudPlanningNode::goalCallback, this);
        
        // 发布路径
        path_pub_ = nh_.advertise<nav_msgs::Path>("/planning/path", 1, true);
        
        ROS_INFO("PointCloud Planning Node initialized");
    }
    
    ~PointCloudPlanningNode() {}
    
private:
    void mapCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
        ROS_INFO("Received point cloud map with %d points", cloud_msg->width * cloud_msg->height);
        
        // 设置点云地图
        if (planner_->setPointCloud(cloud_msg)) {
            ROS_INFO("Point cloud map set successfully");
            map_received_ = true;
        } else {
            ROS_ERROR("Failed to set point cloud map");
        }
    }
    
    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal_msg) {
        if (!map_received_) {
            ROS_WARN("No map received yet, cannot plan path");
            return;
        }
        
        ROS_INFO("Received goal: (%.2f, %.2f)", goal_msg->pose.position.x, goal_msg->pose.position.y);
        
        // 获取当前位置
        geometry_msgs::PoseStamped current_pose;
        if (!getCurrentPose(current_pose)) {
            ROS_ERROR("Failed to get current pose, cannot plan path");
            return;
        }
        
        // 规划路径
        nav_msgs::Path path;
        if (planner_->plan(current_pose, *goal_msg, path)) {
            ROS_INFO("Path planned successfully with %zu waypoints", path.poses.size());
            path_pub_.publish(path);
        } else {
            ROS_ERROR("Failed to plan path");
        }
    }
    
    bool getCurrentPose(geometry_msgs::PoseStamped& pose) {
        try {
            // 尝试从TF获取当前位置
            tf::StampedTransform transform;
            tf_listener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(1.0));
            tf_listener_.lookupTransform("map", "base_link", ros::Time(0), transform);
            
            pose.header.frame_id = "map";
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = transform.getOrigin().x();
            pose.pose.position.y = transform.getOrigin().y();
            pose.pose.position.z = transform.getOrigin().z();
            pose.pose.orientation.x = transform.getRotation().x();
            pose.pose.orientation.y = transform.getRotation().y();
            pose.pose.orientation.z = transform.getRotation().z();
            pose.pose.orientation.w = transform.getRotation().w();
            
            return true;
        } catch (tf::TransformException& ex) {
            ROS_ERROR("Transform error: %s", ex.what());
            
            // 如果无法获取TF，使用默认位置（原点）
            pose.header.frame_id = "map";
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = 0.0;
            pose.pose.position.y = 0.0;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.w = 1.0;
            
            ROS_WARN("Using default pose at origin");
            return true;
        }
    }
    
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    ros::Subscriber map_sub_;
    ros::Subscriber goal_sub_;
    ros::Publisher path_pub_;
    tf::TransformListener tf_listener_;
    std::shared_ptr<auto_navigation::planning::PointCloudPlanner> planner_;
    bool map_received_ = false;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_planning_node");
    
    PointCloudPlanningNode node;
    
    ros::spin();
    
    return 0;
} 